#!/usr/bin/env python

#Daniel Cook - 
#COMP4766 - A1
  
import roslib
roslib.load_manifest('comp4766_a1')
import rospy
import math
# Add other import statements if necessary

# Import the formats needed for used services and messages
from turtlesim.srv import SetPen
from turtlesim.msg import Color, Pose

def pose_callback(pose):
    i = 255-((math.sqrt(math.pow(pose.x-5.5444, 2) + math.pow(pose.y-5.5444, 2))*45.992352644)*0.75)
    pen(i,i,i,20,0)

if __name__ == '__main__':
    # The pen handle (set below) will be used in 'pose_callback' so we make it
    # global.  Another way of handling this would be to wrap this functionality
    # into a class.
    global pen

    # Initialize this node.
    rospy.init_node('init')

    # Create a handle for calling the set_pen service (e.g. tutorial 15)
    pen = rospy.ServiceProxy('/turtle1/set_pen', SetPen);

    # Subscribe to /turtle1/pose and set 'pose_callback' as our callback
    # (e.g. tutorial 12).
    rospy.Subscriber("/turtle1/pose", Pose, pose_callback)

    # This prevents the node from exiting, which is necessary since we want to
    # continue receiving callbacks when the turtle moves.
    rospy.spin()
